﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using motion.Properties;
using Mahanet.Abyss.PathCalculation;

namespace motion
{
  public class ImageTo3DTranslator
  {
    private static double AngleHeight = 480d / 640d * Settings.Default.WidthDegrees;
    private static double ZeroAngle =
        AngleHeight * ((float)Settings.Default.CameraDistanceAboveTurret / (Settings.Default.FieldRadius * (480d / 640d)));

    public static Coordinate3D GetDetectionPointIn3DSpace(IMotionDetector detectorX, IMotionDetector detectorZ)
    {
      return ImageTo3DTranslator.GetPointIn3DSpace(detectorX.AngleX.Value, detectorZ.AngleX.Value, detectorZ.AngleY.Value);
    }

    public static Coordinate3D GetScreenPointIn3DSpace(int x, int z, int y, CameraWindow camWindow)
    {
      double detectionXAngle = ((double)x) / (((double)camWindow.Width) / Settings.Default.WidthDegrees);
      double detectionZAngle = ((double)z) / (((double)camWindow.Width) / Settings.Default.WidthDegrees);
      double detectionYAngle = ((double)(camWindow.Height - y)) / (((double)camWindow.Height) / AngleHeight);

      return ImageTo3DTranslator.GetPointIn3DSpace(GetDetectionAngleX(detectionXAngle),
                                              GetDetectionAngleX(detectionZAngle),
                                              GetDetectionAngleY(detectionYAngle));
    }

    public static Coordinate3D GetScreenPointIn3DSpaceP(
      int xTopLeft,
      int zTopLeft,
      int yTopLeft,
      int windWidth,
      int windHeight)
    {
      // hfov - half angle of field-of-view, always non-negative
      // camDist - camera distance from zero
      // fRadius - horizontal field radius, un any unit. 
      //            Basically half the number of horizontal units in view.
      //
      // top left values are measured from the top left corner of the viewing window
      // observed values are measured from center of the viewing window
      // tan(hfov) = fRadius / camDist;
      // tan(x-angle) = x_obs / camDist; 
      // tan(x-angle) = x_obs * tan(hfov) / fRadius
      // tan(z-angle) = z_obs * tan(hfov) / fRadius
      // calculate:
      // tanx = tan(x-angle), tanz = tan(z-angle);

      // prefix rad_ for radians
      double rad_hfov = MathUtils.ConvertDegreesToRadians(Settings.Default.WidthDegrees / 2.0f);
      double tan_hfov = Math.Tan(rad_hfov);
      double fRadius = windWidth / 2.0;

      // shift top-left values to center
      double x_obs = xTopLeft - (windWidth / 2.0);
      double z_obs = zTopLeft - (windWidth / 2.0);

      // Z-camera should be located to the right of X-camera. Both cameras capture TopLeft frames.
      // It makes the positive horizontal direction of the Z-camera to point towards the Z-camera itself.
      // Reverse the Z axis, so that positive directions of both axis would point 
      // from the camera, outwards.
      z_obs = -z_obs;

      // tan(x-angle) = (z_obs - z_real) / x_real)
      // tan(z-angle) = (x_obs - x_real) / z_real)
      // this calculation assumes that dx, dz are non-negative
      // actually need to multiply dx, dz by a +- 1.
      // determine their positive transformation sign and add to equation

      // after long calculation, do it on paper
      // x_real = (x_obs - tanx*z_obs) / (1 - tanx*tanz) 
      // z_real = (z_obs - tanz*x_obs) / (1 - tanz*tanx)


      // Sign +- of the small angle that detector ray forms with the axis.
      
      int signX = Math.Sign(z_obs);
      int signZ = Math.Sign(x_obs);
      // if ray is on axis, sign should be neutral (+1)
      signX = signX == 0 ? 1 : signX;
      signZ = signZ == 0 ? 1 : signZ;
      //now lose the sign, calculate only for normal triangles.
      x_obs = Math.Abs(x_obs);
      z_obs = Math.Abs(z_obs);
      
      double tanx = x_obs * tan_hfov / fRadius;
      double tanz = z_obs * tan_hfov / fRadius;
      
      // Would like to calculate simple, positive quantities that form the triangles
      // - deltaz, x_real, tanx
      // - deltax, z_real, tanz
      // Where dx == x_obs - x_real, dz == z_obs - z_real
      // for dx, dz to be non-negative, need to look at the
      // the signs of anglex, anglez.
      // draw the four options on paper and determine yourself.
      int signDX = signZ * (-1) * signX;
      int signDZ = signX * (-1) * signZ;
      int signDXZ = signDX * signDZ;

      //double x_real = (x_obs - (tanx * z_obs)) / (1 - (tanx * tanz));

      double z_real = ( (signDXZ * z_obs) - (tanz * signDX * x_obs)) / 
                      (signDXZ - (tanz * tanx));

      double x_real = ((signDXZ * x_obs) - (tanx * signDZ * z_obs)) /
                      (signDXZ - (tanx * tanz));

      // now, use sign information to calculate real X,Z values
      x_real *= signZ;
      z_real *= signX;
      // don't forget to reverse back z-value, for the reversed axis
      z_real = -z_real;

      

      //----------------------------------------------------------------


      //// Calculate the absolute value of the prefixed X and Z values
      //double absolutePrefixedX = Math.Abs(x_obs);
      //double absolutePrefixedZ = Math.Abs(z_obs);

      //// Calculate the delta for both X and Z
      //double zDelta = (tanx * absolutePrefixedX + signX * tanx * tanz * absolutePrefixedZ) /
      //                (1 - signX * signZ * tanx * tanz);

      //double xDelta = tanz * absolutePrefixedZ + signZ * tanz * zDelta;


      //// Calculate the "direction" sign
      //float directionSignX = 1;

      //if (x_obs < 0)
      //{
      //  directionSignX = -1;
      //}

      //float directionSignZ = 1;

      //if (z_obs < 0)
      //{
      //  directionSignZ = -1;
      //}

      //// Calculate the correct X and Z values using the deltas found
      //double correctXVal = x_obs + directionSignX * signX * xDelta;
      //double correctZVal = z_obs + directionSignZ * signZ * zDelta;

      //// Calculate the target's height. NOTE: This should be tested!
      ////double objectHeight = (Settings.Default.CameraDistanceFromTurret + correctXVal) * Math.Tan(MathUtils.ConvertDegreesToRadians(detectionYAngle * (480d / 640d)));
      //double objectHeight = 0;

      //// Add the camera's height above the ground.
      //if (Settings.Default.IsUsingCameraHeightVar)
      //{
      //  objectHeight += Settings.Default.CameraDistanceAboveTurret;
      //}

      //return new Coordinate3D(correctXVal, objectHeight, correctZVal);
      //----------------------------------------------------------------

      return new Coordinate3D(x_real, 0, z_real);
    }

    public static Coordinate3D ToMeters(Coordinate3D worldPoint_px, int viewWidth_px, double cameraDistance_M)
    {
      // tan(half-field-of-view) = field_radius / camera_distance
      // ex. field_radius in pixels: 640/2 (half the width of teh viewing window).
      // conversion ratio pixels-per-meter: 
      double fieldRadius_px = viewWidth_px / 2.0;
      double hfov_rad = MathUtils.ConvertDegreesToRadians(Settings.Default.WidthDegrees / 2.0);
      double cameraDistance_px = fieldRadius_px / Math.Tan(hfov_rad);
      double meter_per_pixel = cameraDistance_M / cameraDistance_px;
      //  TODO - verrical resolution may be different
      return new Coordinate3D(
        worldPoint_px.X * meter_per_pixel,
        worldPoint_px.Y * meter_per_pixel,
        worldPoint_px.Z * meter_per_pixel);
    }

    public static void GetWorldPointOnScreen(double x, double y, double z,
                                       out Coordinate2D camera1Coordinate, out Coordinate2D camera2Coordinate,
                                       CameraWindow cameraWindow1, CameraWindow cameraWindow2)
    {
      double height = y;

      if (Settings.Default.IsUsingCameraHeightVar)
      {
        height -= Settings.Default.CameraDistanceAboveTurret;
      }

      double xResult = cameraWindow1.Width / 2 + x / Settings.Default.FieldRadius * cameraWindow1.Width / 2;
      double zResult = cameraWindow2.Width / 2 + z / Settings.Default.FieldRadius * cameraWindow2.Width / 2;

      // Calculate the y result and point it down, to fit the screen/cameraWindow coordinate system
      double yResultPointedUp = height / (2d * Settings.Default.FieldRadius * (480d / 640d)) * (double)cameraWindow1.Height;
      double yResult = cameraWindow1.Height - yResultPointedUp;

      camera1Coordinate = new Coordinate2D(xResult, yResult);
      camera2Coordinate = new Coordinate2D(zResult, yResult);
    }

    private static Coordinate3D GetPointIn3DSpace(double detectionXAngle, double detectionZAngle, double detectionYAngle)
    {
      // Calculate the prefixed X and Z values
      //double preFixedXVal = (detectionXAngle / (Settings.Default.WidthDegrees / 2f)) * Settings.Default.FieldRadius;
      //double preFixedZVal = (detectionZAngle / (Settings.Default.WidthDegrees / 2f)) * Settings.Default.FieldRadius;
      double detectionXAngleRadians = MathUtils.ConvertDegreesToRadians(detectionXAngle);
      double detectionZAngleRadians = MathUtils.ConvertDegreesToRadians(detectionZAngle);
      double preFixedXVal = Settings.Default.CameraDistanceFromTurret /
                            Math.Tan(detectionXAngleRadians);
      double preFixedZVal = Settings.Default.CameraDistanceFromTurret /
                            Math.Tan(detectionXAngleRadians);

      // Calculate the sign (+, -) with which to multiply parts of the delta calculation
      int signX = 1;
      int signZ = 1;

      if (preFixedZVal < 0)
      {
        signX = -1;
      }

      if (preFixedXVal < 0)
      {
        signZ = -1;
      }

      // Calculating the tan of the both angles
      double tanAlpha = Math.Tan(MathUtils.ConvertDegreesToRadians(Math.Abs(detectionZAngle)));
      double tanBeta = Math.Tan(MathUtils.ConvertDegreesToRadians(Math.Abs(detectionXAngle)));

      // Calculate the absolute value of the prefixed X and Z values
      double absolutePrefixedX = Math.Abs(preFixedXVal);
      double absolutePrefixedZ = Math.Abs(preFixedZVal);

      // Calculate the delta for both X and Z
      double zDelta = (tanAlpha * absolutePrefixedX + signX * tanAlpha * tanBeta * absolutePrefixedZ) /
                      (1 - signX * signZ * tanAlpha * tanBeta);

      double xDelta = tanBeta * absolutePrefixedZ + signZ * tanBeta * zDelta;


      // Calculate the "direction" sign
      float directionSignX = 1;

      if (preFixedXVal < 0)
      {
        directionSignX = -1;
      }

      float directionSignZ = 1;

      if (preFixedZVal < 0)
      {
        directionSignZ = -1;
      }

      // Calculate the correct X and Z values using the deltas found
      double correctXVal = preFixedXVal + directionSignX * signX * xDelta;
      double correctZVal = preFixedZVal + directionSignZ * signZ * zDelta;

      // Calculate the target's height. NOTE: This should be tested!
      double objectHeight = (Settings.Default.CameraDistanceFromTurret + correctXVal) * Math.Tan(MathUtils.ConvertDegreesToRadians(detectionYAngle * (480d / 640d)));

      // Add the camera's height above the ground.
      if (Settings.Default.IsUsingCameraHeightVar)
      {
        objectHeight += Settings.Default.CameraDistanceAboveTurret;
      }

      return new Coordinate3D(correctXVal, correctZVal, objectHeight);
    }

    private static double GetDetectionAngleX(double rawDetectionAngle)
    {
      if (rawDetectionAngle >= (Settings.Default.WidthDegrees / 2f))
      {
        return (rawDetectionAngle - (Settings.Default.WidthDegrees / 2f));
      }
      else
      {
        return (0 - ((Settings.Default.WidthDegrees / 2f) - rawDetectionAngle));
      }
    }

    private static double GetDetectionAngleY(double rawDetectionAngle)
    {
      if (Settings.Default.IsCameraAtMiddleOfYAxis)
      {
        if (rawDetectionAngle >= (AngleHeight / 2f))
        {
          return (rawDetectionAngle - (AngleHeight / 2f));
        }
        else
        {
          return (0 - ((AngleHeight / 2f) - rawDetectionAngle));
        }
      }
      else if (Settings.Default.IsUsingCameraHeightVar)
      {
        if (rawDetectionAngle >= ZeroAngle)
        {
          return (rawDetectionAngle - ZeroAngle);
        }
        else
        {
          return (0 - (ZeroAngle - rawDetectionAngle));
        }
      }
      else
      {
        return (rawDetectionAngle);
      }
    }
  }
}
